#include "System.h"
#include <algorithm>
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"

System::System()
{
}

System::~System()
{
	Deinitialize();
}

void System::Initialize()
{
	m_broadphase = new btDbvtBroadphase();
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_solver = new btSequentialImpulseConstraintSolver;
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
}

void System::Deinitialize()
{   
	while (m_bodies.begin() != m_bodies.end())
	{
		RigidBody* body = *m_bodies.begin();
		RemoveRigidBody(body);
	}

	delete m_dynamicsWorld;
    delete m_solver;
    delete m_dispatcher;
    delete m_collisionConfiguration;
    delete m_broadphase;
}

void System::setGravity( float x, float y, float z )
{
	m_dynamicsWorld->setGravity(btVector3(x, y, z));
}

void System::setGravity( Maths::Vector3D& gravity )
{
	m_dynamicsWorld->setGravity(btVector3(gravity.X(), gravity.Y(), gravity.Z()));
}

void System::Evaluate( float timeChange )
{
	m_dynamicsWorld->stepSimulation(timeChange);
}

void System::PrintObjects()
{
	for (int j=m_dynamicsWorld->getNumCollisionObjects()-1; j>=0 ;j--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[j];
		btRigidBody* body = btRigidBody::upcast(obj);

		if (body && body->getMotionState())
		{
			btTransform trans;
			body->getMotionState()->getWorldTransform(trans);
			printf("body %d world pos = %f,%f,%f\n",j, float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ()));
		}
	}
}

void System::AddRigidBody( RigidBody* body )
{
	btRigidBody* btBody = body->getRigidBody();
	m_dynamicsWorld->addRigidBody(btBody);
	m_bodies.push_back(body);
	body->setSystem(this);
}

void System::RemoveRigidBody( RigidBody* body )
{
	btRigidBody* btBody = body->getRigidBody();
	m_dynamicsWorld->removeRigidBody(btBody);
	m_bodies.remove(body);
	body->setSystem(0);
}

bool System::RayCast( Maths::Vector3D& start, Maths::Vector3D& end, void** pointer )
{
	m_dynamicsWorld->updateAabbs();
	m_dynamicsWorld->computeOverlappingPairs();
	btVector3 btRayFrom(start.X(), start.Y(), start.Z());
	btVector3 btRayTo(end.X(), end.Y(), end.Z());
	btCollisionWorld::ClosestRayResultCallback rayCallback(btRayFrom, btRayTo);
    m_dynamicsWorld->rayTest(btRayFrom, btRayTo, rayCallback);
    if (rayCallback.hasHit())
    {
        *pointer = rayCallback.m_collisionObject->getUserPointer();
		return true;
	}
	return false;
}

bool System::RayCastGetPoint( Maths::Vector3D& start, Maths::Vector3D& end, void** pointer, Maths::Vector3D& point )
{
	m_dynamicsWorld->updateAabbs();
	m_dynamicsWorld->computeOverlappingPairs();
	btVector3 btRayFrom(start.X(), start.Y(), start.Z());
	btVector3 btRayTo(end.X(), end.Y(), end.Z());
	btCollisionWorld::ClosestRayResultCallback rayCallback(btRayFrom, btRayTo);
    m_dynamicsWorld->rayTest(btRayFrom, btRayTo, rayCallback);
    if (rayCallback.hasHit())
    {
		point = Maths::Vector3D(rayCallback.m_hitPointWorld.x(), rayCallback.m_hitPointWorld.y(), rayCallback.m_hitPointWorld.z());
        *pointer = rayCallback.m_collisionObject->getUserPointer();
		return true;
	}
	return false;
}
//void System::setGlobalDamping( float damping )
//{
//}
//
//float System::getGlobalDamping()
//{
//	return 0.0f;
//}
//
//void System::setGlobalAcceleration( Maths::Vector3D& forces )
//{
//}
//
//const Maths::Vector3D& System::getGlobalAcceleration()
//{
//}
